#include <iostream>
#include <Eigen/Dense>
#include "rotation.h"
#include "../include/class.h"
using namespace Eigen;
using namespace std;

/*
* IMUDATA::construct function
* */
IMUDATA::IMUDATA(Vector3d newGyroData,
                 Vector3d newAccelData,
                 double newTime) :
        gyroData(newGyroData),
        accelData(newAccelData),
        time(newTime){

}

/*
 * IMUDATA:: default construct function
 * */
IMUDATA::IMUDATA():
        gyroData(Vector3d(0, 0,0)),
        accelData(Vector3d(0, 0, 0)),
        time(0){

}


/*
 * IMUDATA:: copy construct function
 * */
IMUDATA::IMUDATA(const IMUDATA &imuData1):
        gyroData(imuData1.gyroData),
        accelData(imuData1.accelData),
        time(imuData1.time){

}

/*
 * IMUDATA:: destruction function
 * */
IMUDATA::~IMUDATA(){

}
/*
 * showIMUDATA
 * */
void IMUDATA::showIMUDATA() const {
    cout << "gyroFront:" << gyroData[0] << endl;
    cout << "gyroRight:" << gyroData[1] << endl;
    cout << "gyroDown:" << gyroData[2] << endl;
    cout << "accelFront:" << accelData[0] << endl;
    cout << "accelRight:" << accelData[1] << endl;
    cout << "accelDown:" << accelData[2] << endl;
    cout << "time:" << time << endl;
}











/*
 * State::construct function
 * */
State::State(Vector3d new_att,
             Vector3d new_vel,
             Vector3d new_pos,
             double new_time) :
        att_quat(Rotation::euler2quaternion(new_att)),
        vel(new_vel),
        pos(new_pos),
        time(new_time){

}

/*
 * State::construct function
 * */
State::State(Quaterniond new_att_quat,
             Vector3d new_vel,
             Vector3d new_pos,
             double new_time) :
        att_quat(new_att_quat),
        vel(new_vel),
        pos(new_pos),
        time(new_time){

}

/*
 * State::construct function
 * */
State::State(Matrix3d new_att_mat,
             Vector3d new_vel,
             Vector3d new_pos,
             double new_time) :
        att_quat(Rotation::matrix2quaternion(new_att_mat)),
        vel(new_vel),
        pos(new_pos),
        time(new_time){

}


/*
 * State::default construct function
 * */
State::State():State(Rotation::matrix2quaternion(Matrix3d::Zero()),
                     Vector3d(0, 0, 0),
                     Vector3d(0, 0, 0),
                     0){

}

/*
 * State:: copy construct function
 * */
State::State(const State &state1) :
        att_quat(state1.att_quat),
        vel(state1.vel),
        pos(state1.pos),
        time(state1.time){

}

/*
 * State:: destruct function
 * */
State::~State(){

}


/*
 * State:: show function in radian
 * */
void State::showStateInRadian() const {
    Vector3d att = get_att();
    cout << "The time is:" << time << endl;
    cout << "Attitude[横滚，俯仰，航向] is:" << "["
         << att[0] << ", "
         << att[1] << ", "
         << att[2] << "]" << endl;
    cout << "Velocity[北，东，地] is:" << "["
         << vel[0] << ", "
         << vel[1] << ", "
         << vel[2] << "]" << endl;
    cout << "pos[纬度，经度，高度] is:" << "["
         << pos[0] << ", "
         << pos[1] << ", "
         << pos[2] << "]" << endl;
}

/*
 * State:: show function in angle
 * */
void State::showStateInAngle() const {
    Vector3d att = get_att();
    cout << "The time is:" << time << endl;
    cout << "Attitude[横滚，俯仰，航向] is:" << "["
         << att[0] / M_PI * 180 << ", "
         << att[1] / M_PI * 180 << ", "
         << att[2] / M_PI * 180 << "]" << endl;
    cout << "Velocity[北，东，地] is:" << "["
         << vel[0] << ", "
         << vel[1] << ", "
         << vel[2] << "]" << endl;
    cout << "pos[纬度，经度，高度] is:" << "["
         << pos[0] / M_PI * 180 << ", "
         << pos[1] / M_PI * 180 << ", "
         << pos[2] << "]" << endl;
}

/*
 * State: change state
 * */
void State::changeState(Vector3d new_att,
                        Vector3d new_vel,
                        Vector3d new_pos,
                        double new_time) {
    att_quat = Rotation::euler2quaternion(new_att),
            vel = new_vel;
    pos = new_pos;
    time = new_time;
}

/*
 * State: change state
 * */
void State::changeState(Quaterniond new_att_quat,
                        Vector3d new_vel,
                        Vector3d new_pos,
                        double new_time) {
    att_quat = new_att_quat,
            vel = new_vel;
    pos = new_pos;
    time = new_time;
}




/*
* GNSSDATA::construct function
* */
GNSSDATA::GNSSDATA(Vector3d position,
                   Vector3d positionStd,
                   double time):
        position(position),
        positionStd(positionStd),
        time(time){

}
/*
 * GNSSDATA:: default construct function
 * */
GNSSDATA::GNSSDATA():
        GNSSDATA(Vector3d(0, 0, 0), Vector3d(0, 0, 0), 0){

}


/*
 * GNSSDATA:: copy construct function
 * */
GNSSDATA::GNSSDATA(const GNSSDATA &gnssData1):
        position(gnssData1.position),
        positionStd(gnssData1.positionStd),
        time(gnssData1.time){

}

/*
 * GNSSDATA:: destruction function
 * */
GNSSDATA::~GNSSDATA(){

}
/*
 * showGNSSDATA
 * */
void GNSSDATA::showGnssData() const {
    cout << "latitude:" << position[0] << endl;
    cout << "longitude:" << position[1] << endl;
    cout << "height:" << position[2] << endl;
    cout << "latitudeStd:" << positionStd[0] << endl;
    cout << "longitudeStd:" << positionStd[1] << endl;
    cout << "heightStd:" << positionStd[2] << endl;
    cout << "time:" << time << endl;
}



